/**
*@Created by YChienHung on 20-09-26.
*/

/**
 * achieve:
 * 1.基本完成了识别的任务
 * 2.实现轮廓的跟踪
 *
 *
 * issue:
 * 1.计算速度的频率不高
 */

/**
 * About Issue 1
 * 1.无法判断红块哪个与哪个是一组的，很容易出现判断错误
 *
 *
 * 解决方案：
 * 1.计算两个红快的速度，之后进行判断，得出最有可能的一个速度输出
 * 2.
 */

#include <iostream>
#include <cmath>
#include "opencv2/opencv.hpp"

#define WINDWOS_NAME "result3"
#define DELAY_TIME 20.0
#define TIMECOUNT 10


using namespace std;
using namespace cv;

float s_Speed = 0;           //角速度


double s_fWidth = 0;
bool s_bReady = false;
int s_iCount = 3;


int main(int argc, char *args[]) {
    Mat img_red;
    Mat frame;            //视频的数据流
    VideoCapture cap = VideoCapture("3.mp4");

    String str_speed;               //显示速度的文本
    ostringstream ostr;             //格式化数据流


    int stCenterXpt;
    uint16_t usSideLength;

    uint8_t ucCount = 0;
    double angle;

    if (cap.isOpened()) {
        for (;;) {
            cap >> frame;
            if (frame.empty())
                break;

            //B G R
            vector<Mat> channels;
            split(frame, channels);  //分离色图像色彩通道，则三通道图像成为三个单通道图像
            img_red = channels.at(2);

            //二值化处理
            threshold(img_red, img_red, 100, 255, THRESH_BINARY);   //根据直方图来计算阈值

            //处理杂块
            Mat kernel = getStructuringElement(MORPH_RECT, Size(30, 30));
            erode(img_red, img_red, kernel);  //腐蚀
            dilate(img_red, img_red, kernel); //膨胀

            //寻找轮廓
            vector<vector<Point>> contours;
            vector<Vec4i> hierarchy;

            findContours(img_red, contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE, Point());

            if (contours.size() == 2 && s_iCount == 3) {
                ucCount = 0;
                s_bReady = true;
            }

            s_iCount = contours.size();

            for (auto &contour : contours) {

                //绘制轮廓的最小外结矩形
                RotatedRect rect = minAreaRect(contour);
                Point2f P[4];
                rect.points(P);

                int width = abs(P[1].x - P[3].x);
                int height = abs(P[1].y - P[3].y);

                //77 29
                if (height < 50 && width < 100) {
                    //上面那个小框框,标记中心用的
                    stCenterXpt = P[1].x / 2 + P[3].x / 2;
                    line(frame, Point(stCenterXpt, 0), Point(stCenterXpt, 500), Scalar(0, 0, 255), 2);
                } else {
                    usSideLength = height;

                    //绘制图形边框
                    for (size_t j = 0; j < 4; j++)
                        line(frame, P[j], P[(j + 1) % 4], Scalar(255, 255, 0), 2);

                    //找到右边的那一个，反正就用一次，肯定没错的
                    if (contours.size() == 3 && (P[1].x / 2 + P[3].x / 2) > stCenterXpt) {
                        s_fWidth = width;
                    }
                }
            }

            ucCount++;

            if (ucCount == TIMECOUNT && s_bReady) {

                angle = acos(s_fWidth / usSideLength) * 180 / M_PI;

                //这里要注意c/c++的计算法则, 否则很容易出错
                s_Speed = fabs(angle - 90) * 1000 / (DELAY_TIME * TIMECOUNT);

                if (isnan(s_Speed))
                    s_Speed = 0;
                ucCount = 0;
                s_bReady = false;
            }

            ostr.str("");//清空数据流, 重复利用
            ostr << "speed: " << s_Speed << " Angle Per Second";
            str_speed = ostr.str();

            putText(frame, str_speed, Point(0, 50), FONT_HERSHEY_COMPLEX, 2, Scalar(255, 255, 255), 2);

            //显示图像
            imshow(WINDWOS_NAME, frame);

            uint8_t key = waitKey(DELAY_TIME);

            if (key == 'q' || key == 'Q')
                break;
        }
    }

    cap.release();// 释放视频句柄
    destroyAllWindows();//关闭所有窗口
    return 0;
}